package com.yhy.wxzn.wcs.device.rgv;


import com.yhy.wxzn.common.utils.SpringUtil;
import com.yhy.wxzn.common.utils.WcsUtils;
import com.yhy.wxzn.wcs.device.plc.bean.FourCarPlc;
import com.yhy.wxzn.wcs.device.plc.bean.XJPHoistPlc;
import com.yhy.wxzn.wcs.device.plc.task.PlcTaskThread;
import com.yhy.wxzn.wcs.device.rgv.bean.*;
import com.yhy.wxzn.wcs.device.rgv.core.avoid.AvoidCentralManagerThread;
import com.yhy.wxzn.wcs.device.rgv.core.avoid.AvoidHandler;
import com.yhy.wxzn.wcs.device.rgv.core.lock.LockNodeManager;
import com.yhy.wxzn.wcs.device.rgv.core.plc.ToLifer;
import com.yhy.wxzn.wcs.device.rgv.core.plc.ToRgv;
import com.yhy.wxzn.wcs.device.rgv.enums.*;
import com.yhy.wxzn.wcs.device.rgv.exception.ActivelyAvoidFailureException;
import com.yhy.wxzn.wcs.device.rgv.exception.AddLockTimeOutException;
import com.yhy.wxzn.wcs.device.rgv.exception.NodeStatusException;
import com.yhy.wxzn.wcs.device.rgv.exception.PalletUpStatusException;
import com.yhy.wxzn.wcs.entity.WcsFourWayTask;
import com.yhy.wxzn.wcs.entity.WcsTask;
import com.yhy.wxzn.wcs.enums.TaskType;
import com.yhy.wxzn.wcs.service.impl.WcsFourWayTaskServiceImpl;
import com.yhy.wxzn.wcs.service.impl.WcsMapInfoServiceImpl;
import com.yhy.wxzn.wcs.service.impl.WcsTaskServiceImpl;
import com.yhy.wxzn.wcs.utils.TaskUtils;
import lombok.extern.slf4j.Slf4j;

import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.Set;

import static com.yhy.wxzn.wcs.SystemInit.Lifter_Connection;
import static com.yhy.wxzn.wcs.device.rgv.FourWayRgvTaskExecuteThread.RGV_PLC_THREAD_LOCAL;
import static com.yhy.wxzn.wcs.device.rgv.FourWayRgvTaskExecuteThread.RGV_THREAD_LOCAL;

@Slf4j
public class PathExecuteHandler {
    private WcsTask task;
    private FourWayRgv rgv;

    private WcsMapInfoServiceImpl mapInfoService = SpringUtil.getBean(WcsMapInfoServiceImpl.class);
    private WcsTaskServiceImpl taskService = SpringUtil.getBean(WcsTaskServiceImpl.class);

    public PathExecuteHandler(WcsTask task) {
        this.task = task;
        this.rgv = RGV_THREAD_LOCAL.get();
    }


    public void executePath(List<Route> routes) {
        for (Route route : routes) {
            if (task.getTaskType() != TaskType.IN && task.getTaskType() != TaskType.OUT && Objects.equals(route.getStart().getNodeType(),NodeType.L)) {
                callLifer2(new Lifter(route.getStart()),route.getStart().getZ(),rgv);
            }

            removeIdleRgv(route.getPath());


            Fragment.buildFragments(route.getPath()).forEach(fragment -> {
                boolean retry = true;
                while (retry) {
                    try {
                        handleFragment(fragment);
                        retry = false;
                    } catch (NodeStatusException e) {
                        log.info("{}等待节点可通过", rgv.getName());
                        WcsUtils.sleep(1000);
                    }
                }
            });

        }
    }

    private void removeIdleRgv(List<Node> path) {
        ArrayList<FourWayRgv> needAvoidRgvs = new ArrayList<>();
        for (int i = 1; i < path.size(); i++) {
            Node n = path.get(i);
            if (LockNodeManager.checkNodesLocked(n)) {
                Object obj = LockNodeManager.getLockNodeObj(n);
                if (obj == rgv) continue;
                if (obj instanceof FourWayRgv){
                    FourWayRgv targetRgv = (FourWayRgv) obj;
                    needAvoidRgvs.add(targetRgv);
                    if (!Objects.equals(targetRgv.getRgvStatus(), RgvStatus.READY))
                        return;
                }
            }
        }

        needAvoidRgvs.forEach(
                needAvoidRgv -> {
                    Node curLocation = needAvoidRgv.getCurLocation();
                    List<Node> map = mapInfoService.getNodes(curLocation, curLocation);
                    Node nearestAvoidNode = new AvoidHandler(map).getNearestAvoidNode2(needAvoidRgv);

                    if (nearestAvoidNode == null) {
                        String msg = String.format("任务id:%s,%s主动避让%s时，无法找到一个合适的避让点，重新路径规划", task.getTaskId(), needAvoidRgv.getName(), rgv.getName());
                        log.info(msg);
                        throw new ActivelyAvoidFailureException();
                    }

                    generateAvoidTask(needAvoidRgv, nearestAvoidNode);
                });
    }

    private void generateAvoidTask(FourWayRgv needAvoidRgv, Node nearestAvoidNode) {
        TaskType taskType = AvoidCentralManagerThread.ensureTaskType(needAvoidRgv);
        WcsTask task = TaskUtils.generateRgvTask(taskType, null, nearestAvoidNode, PriorityCodes.AVOID, rgv);
        taskService.save(task);
    }



    private void handleFragment(Fragment fragment) {
        rgv.checkIntercept("handleFragment");
        printPath(fragment);
        //验证接驳点是否有货，举升状态等
        if (!checkNodes(fragment.getNodes())) throw new NodeStatusException();


        if (Lifter_Connection){
            waitingInportTaskLifterToTartgetLayer(fragment);


            waitingConnectionLifter(fragment);
        }

        callLifer1(fragment);

        //添加需要避让的车辆
        addNeedAvoidRgv(fragment);

        //等待能上锁
        if (Lifter_Connection){
            beforeAddLockCheckForConnectionLifter(fragment);
        }else {
            beforeAddLockCheck(fragment);
        }



        //加锁
        if (Lifter_Connection){
            addLockForConnectionLifter(fragment,rgv);
        }else {
            LockNodeManager.addLockedNodes(LockNodeManager.newSetByList(fragment.getNodes()), rgv);
        }


        //验证接驳点是否有货，举升状态等，出现异常需要解锁并结束，等待重试
        if (!checkNodes(fragment.getNodes())) {
            rgv.getCurLocationAndRefreshRgvLocation();
            throw new NodeStatusException();
        }

        //换向
        ToRgv.getInstance(rgv).checkDirectAndCommutate(task,fragment.getReversingDirection());


        //执行
        int toRgvTaskNo = FourCarPlc.getAutoIncrementRgvTaskNo();
        long sendTime = System.currentTimeMillis();
        ToRgv.getInstance(rgv).sendRunCommand(fragment, toRgvTaskNo);

        //等待执行结束
        ToRgv.getInstance(rgv).waitRunCommandFinish(toRgvTaskNo, sendTime);

        //解锁
        rgv.getCurLocationAndRefreshRgvLocation();



        //入库，当车车离开后，解锁提升机
        if (Lifter_Connection){
            if (task.getTaskType() == TaskType.IN){
                List<Node> nodes = fragment.getNodes();
                Node start = nodes.get(0);
                if (start.getNodeType() == NodeType.L){
                    LockNodeManager.unlockLifter(new Lifter(start.getX(),start.getY()),task);
                }
            }
        }

    }

    private void waitingInportTaskLifterToTartgetLayer(Fragment fragment) {
        List<Node> nodes = fragment.getNodes();
        Node node = nodes.get(0);
        if (task.getTaskType() == TaskType.IN && task.getRgvProgress() == RgvProgress.RGV_TO_END && node.getNodeType() == NodeType.L){
            while (true){
                rgv.checkIntercept("waitingInportTaskLifterToTartgetLayer");
                ToLifer toLifer = ToLifer.getInstance(new Lifter(node.getX(), node.getY()));
                if (toLifer.isReady()
                        && toLifer.getHoistPlc().getLayer() == node.getZ()
                        && toLifer.getHoistPlc().getIsHaveCargo()
                        //&& LockNodeManager.checkNodeIsLocked(node,task)
                ) break;
                WcsUtils.sleep(1000);
            }
        }
    }


    private void addLockForConnectionLifter(Fragment fragment, FourWayRgv rgv) {
        List<Node> nodes = fragment.getNodes();
        Node lastNode = nodes.get(nodes.size() - 1);

        List<Node> newPath = new ArrayList<>();
        if ((task.getTaskType() == TaskType.IN || task.getTaskType() == TaskType.OUT) && lastNode.getNodeType() == NodeType.L){
            newPath.addAll(nodes);
            newPath.remove(newPath.size() - 1);
        }
        LockNodeManager.addLockedNodes(LockNodeManager.newSetByList(newPath), rgv);
    }

    /**
     * 入库，取货等待，提升机到位，提升机有货
     * 出库，放货等待，提升机到位，提升机没货
     *       出库主动调提升机到当前层，并且
     */
    private void waitingConnectionLifter(Fragment fragment) {
        Node lastNode = fragment.getNodes().get(fragment.getNodes().size() - 1);
        if (lastNode.getNodeType() != NodeType.L) return;


        if (task.getTaskType() == TaskType.IN && task.getRgvProgress() == RgvProgress.RGV_TO_START){
            while (true){
                rgv.checkIntercept("waitingConnectionLifter");
                ToLifer toLifer = ToLifer.getInstance(new Lifter(lastNode.getX(), lastNode.getY()));
                if (toLifer.isReady()
                        && toLifer.getHoistPlc().getLayer() == lastNode.getZ()
                        && toLifer.getHoistPlc().getIsHaveCargo()
                        && LockNodeManager.checkNodeIsLocked(lastNode,task)) break;
                WcsUtils.sleep(1000);
            }
        }else if (task.getTaskType() == TaskType.OUT && task.getRgvProgress() == RgvProgress.RGV_TO_END){
            //调提升机
            PlcTaskThread.executeTask(task);
            while (true){
                rgv.checkIntercept("waitingConnectionLifter");
                ToLifer toLifer = ToLifer.getInstance(new Lifter(lastNode.getX(), lastNode.getY()));
                XJPHoistPlc hoistPlc = toLifer.getHoistPlc();
                if (
                        hoistPlc.getLayer() == lastNode.getZ() && hoistPlc.getTaskStatus() == 1 && hoistPlc.getTaskNoR() == toLifer.getLifterTaskId()
                        && !toLifer.getHoistPlc().getIsHaveCargo()
                        && LockNodeManager.checkNodeIsLocked(lastNode,task)
                ) {
                    toLifer.clearLifterTake();
                    break;}
                WcsUtils.sleep(1000);
            }
        }
    }

    private void printPath(Fragment fragment) {
        List<Node> nodes = fragment.getNodes();
        StringBuilder sb = new StringBuilder();
        for (Node node : nodes) {
            sb.append(node.toString());
            sb.append(",");
        }
        log.info("{} 任务Id:{} 行驶路径{}",rgv.getName(),rgv.getCurTask().getTaskId(),sb.toString());
    }

    private void callLifer1(Fragment fragment) {
        List<Node> nodes = fragment.getNodes();
        Node last = nodes.get(nodes.size() - 1);

        if (!Objects.equals(NodeType.L,last.getNodeType())) return;

        Lifter lifter = new Lifter(last);
        int z = last.getZ();

        if (task.getTaskType() == TaskType.CHARGE
         ||  task.getTaskType() == TaskType.MOVE_LOADED
         ||  task.getTaskType() == TaskType.MOVE_UNLOADED
         ||  task.getTaskType() == TaskType.TRANSFER){
            ToLifer.getInstance(lifter).carCallLifer1(rgv,z);
        }

    }



    private void callLifer2(Lifter lifter, int z, FourWayRgv rgv) {
        ToLifer toLifer = ToLifer.getInstance(lifter);
        int floor = toLifer.getHoistPlc().getLayer();
        if (floor == z) return;

        toLifer.carCallLifer2(rgv,z);
    }

    private void addNeedAvoidRgv(Fragment fragment) {
        List<Node> nodes = fragment.getNodes();

        Set<FourWayRgv> lockObjs = LockNodeManager.getLockRgvsByCoords(nodes);

        lockObjs.remove(rgv);

        rgv.getWaitingForReleaseResource().addAll(lockObjs);
    }


    private void beforeAddLockCheckForConnectionLifter(Fragment fragment) {
        List<Node> nodes = fragment.getNodes();
        Node lastNode = nodes.get(nodes.size() - 1);

        List<Node> newPath = new ArrayList<>();
        if ((task.getTaskType() == TaskType.IN || task.getTaskType() == TaskType.OUT) && lastNode.getNodeType() == NodeType.L){
            newPath.addAll(nodes);
            newPath.remove(newPath.size() - 1);
        }
        int time = 0;
        while (LockNodeManager.checkNodeListLockBy(newPath, rgv) != null) {
            rgv.checkIntercept("小车等待加锁中");
            rgv.checkAvoidIntercept();

            if (time % 10 == 0) log.info("{}小车等待加锁中", rgv.getName());

            WcsUtils.sleep(1000);

            if (time++ == 180) {
                log.info("{}小车等待加锁超时，正在重新路径规划", rgv.getName());
                throw new AddLockTimeOutException();
            }
        }

    }
    private void beforeAddLockCheck(Fragment fragment) {
        int time = 0;
        while (LockNodeManager.checkNodeListLockBy(fragment.getNodes(), rgv) != null) {
            rgv.checkIntercept("小车等待加锁中");
            rgv.checkAvoidIntercept();

            if (time % 10 == 0) log.info("{}小车等待加锁中", rgv.getName());

            WcsUtils.sleep(1000);

            if (time++ == 180) {
                log.info("{}小车等待加锁超时，正在重新路径规划", rgv.getName());
                throw new AddLockTimeOutException();
            }
        }
    }

    private boolean checkNodes(List<Node> nodes) {
        for (int i = 0; i < nodes.size(); i++) {
            Node node = mapInfoService.getNode(nodes.get(i));

            if (node.getNodeStatus().equals(NodeStatus.Y) && RGV_PLC_THREAD_LOCAL.get().getPosition() == Action.顶升.getCmd()) {
                log.info("{}有货，小车无法通行,重新路径规划", node);
                throw new PalletUpStatusException();
            }




            if (task.getTaskType().equals(TaskType.OUT)
                    && node.getNodeType().equals(NodeType.CONNECTION)
                    && (node.getNodeStatus().equals(NodeStatus.Y)
                    //TODO   ||接驳点光电有货
            )
            ) {


                log.info("{}出库任务，接驳点有货，小车等待中", node);
                return false;
            }

        }
        return true;
    }


//    private void unLockNodeExceptCurrentRgvLocation() {
//        Set<Coord> unlockSet = LockNodeManager.getLockCoordByObj(rgv);
//        unlockSet.remove(rgv.getCurLocation());
//        StringBuilder sb = new StringBuilder();
//        for (Coord coord : unlockSet) {
//            sb.append(coord.toString());
//            sb.append(",");
//        }
//        log.info("{}行驶完一段路径，释放节点：{}",rgv.getName(),sb.toString());
//        LockNodeManager.removeLockedNodes(unlockSet, rgv);
  //  }


}
